'''
1、接受tessng向kafka发送的车辆数据,并进行展示

2、单个雷达,可以实时展示雷达的点云图像,并将进行保存

'''

import carla
import numpy as np
import time
from queue import Queue, Empty
from carla import Transform, Rotation, Location
from kafka import KafkaConsumer
import json
from pyproj import Proj
import numpy as np
import cv2
import copy
import open3d as o3d
import os


# 经纬度转xy坐标，在carla中y坐标的值需要取反
rule = Proj("+proj=tmerc +lat_0=40.04752343063863 +lon_0=116.2872228823074 +k=1 +x_0=0 +y_0=0 +datum=WGS84")

# 将雷达的点云保存到队列
def sensor_callback(sensor_data, sensor_queue, sensor_name):
    sensor_queue.put((sensor_data.frame, sensor_data.timestamp, sensor_name, sensor_data))

# 时间格式化
def get_time_stamp(ct):
    local_time = time.localtime(ct)
    data_head = time.strftime("%Y-%m-%d %H-%M-%S", local_time).split(' ')[-1]
    data_secs = (ct - int(ct)) * 1000
    time_stamp = "%s-%03d" % (data_head, data_secs)
    return time_stamp

def _parse_lidar_cb(lidar_data):
    points = np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4'))
    points = copy.deepcopy(points)
    points = np.reshape(points, (int(points.shape[0] / 4), 4))
    return points

# 保存点云数据
def points2pcd(PCD_FILE_PATH, points):

    if os.path.exists(PCD_FILE_PATH):
        os.remove(PCD_FILE_PATH)
    # 写文件句柄
    handle = open(PCD_FILE_PATH, 'a')
    # 得到点云点数
    point_num = points.shape[0]
    # pcd头部（重要）
    handle.write(
        '# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z intensity\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1')
    string = '\nWIDTH ' + str(point_num)
    handle.write(string)
    handle.write('\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0')
    string = '\nPOINTS ' + str(point_num)
    handle.write(string)
    handle.write('\nDATA ascii')

    # 依次写入点
    for i in range(point_num):
        string = '\n' + str(points[i, 0]) + ' ' + str(points[i, 1]) + ' ' + str(points[i, 2]) + ' ' + str(points[i, 3])
        handle.write(string)
    handle.close()

def main():
  
    try:
                     
    #  ======================================跟服务器实现连接=============================================
        client = carla.Client('localhost', 2000)
        client.set_timeout(10.0)
                 
    #  ======================================使用自定义地图=============================================
        world = client.load_world('shangdiwr')

    # =============================天气=================================

        # 控制世界的天气和时间（太阳的位置） 万里无云，没有降雨，太阳的角度为90
        weather = carla.WeatherParameters(
            cloudiness=0.0,  # 0-100  0 是晴朗的天空，100 是完全阴天
            precipitation=0.0,  # 0 表示没有下雨，100 表示大雨
            precipitation_deposits=0.0, # 0 表示道路上没有水坑，100 表示道路完全被雨水覆盖
            wind_intensity=0.0, # 0 表示平静，100 表示强风，风会影响雨向和树叶
            sun_azimuth_angle=0.0, # 太阳方位角，0～360
            sun_altitude_angle=90.0,  # 太阳高度角，90 是中午，-90 是午夜
            fog_density=0.0, # 0～100 表示雾的浓度或厚度，仅影响RGB相机传感器
            fog_distance=0.0, # 雾开始的距离，单位为米
            wetness=0.0, # 0～100 表示道路湿度百分比，仅影响RGB相机传感器
            fog_falloff=0.0, # 雾的密度，0至无穷大，0 表示雾比空气轻，覆盖整个场景，1表示与空气一样，覆盖正常大小的建筑物
            scattering_intensity=0.0, # 控制光线对雾的穿透程度
            mie_scattering_scale=0.0, # 控制光线与花粉或空气等大颗粒的相互作用，导致天气朦胧，光源周围有光晕，0表示无影响
            rayleigh_scattering_scale=0.0331, # 控制光与空气分子等小粒子的相互作用，取决于光波长，导致白天蓝天或晚上红天
            )
        world.set_weather(weather)
        
        
        
        # =============================创建kafka消费者=================================
        topic = 'tess_send_carla_shangdi'
        bootstrap_servers = '106.120.201.126:14576'
        # 创建Kafka消费者实例
        consumer = KafkaConsumer(
            topic,
            bootstrap_servers = bootstrap_servers,
            value_deserializer=lambda x: json.loads(x.decode('utf-8'))  # 指定反序列化函数为JSON解析
        )    
        
        # =============================创建交通管理器对象，并设置仿真的倍数和车辆最大的速度=================================  
    
        traffic_manager = client.get_trafficmanager(8000)
        # traffic_manager.set_global_port(8100)
        traffic_manager.set_global_distance_to_leading_vehicle(2.0)
        traffic_manager.global_percentage_speed_difference(40.0)
        
        if True:
            #设置TM同步模式
            settings = world.get_settings()
            traffic_manager.set_synchronous_mode(True)
            if not  settings.synchronous_mode:
                print("开启同步模式")
                synchronous_master = True
                settings.synchronous_mode = True
                settings.fixed_delta_seconds = 0.1
                world.apply_settings(settings)
            else:
                print("开启异步模式")
                synchronous_master = False
                traffic_manager.set_synchronous_mode(False)
                settings.synchronous_mode = False
                settings.fixed_delta_seconds = 0.1
                world.apply_settings(settings)
        
    # =============================视角坐标=================================
        x, y = rule(116.29220138718945, 40.05073628413568)



        vehicle_trans11 = Transform(Location(x=424.81892146460007, y=-356.75291758771124, z=65.677723), 
                                    Rotation(yaw = 270))
        world.get_spectator().set_transform(vehicle_trans11)  # 设置世界视角
        
        blueprint_library = world.get_blueprint_library()
        
       

   # =======================================雷达==================================

        lidar_x, lidar_y = rule(116.29156843270846,40.05293308929916)

        vehicle_trans_lidar = Transform(Location(x=lidar_x, y=-lidar_y, z=45.677723), 
                                    Rotation(yaw = 347.65-90))
        


        world.debug.draw_string(vehicle_trans_lidar.location, 'X', draw_shadow=False,
                                color=carla.Color(r=255, g=0, b=0), life_time=1500, persistent_lines=True)
     
        lidar_bp_16 = blueprint_library.find('sensor.lidar.ray_cast')  
        
        lidar_bp_16.set_attribute('channels', '16')# 扫描点的通道数量
        lidar_bp_16.set_attribute('upper_fov', '-10')#垂直方向上的视野范围（弧度表示，0到佩）
        lidar_bp_16.set_attribute('lower_fov', '-30')# 
        lidar_bp_16.set_attribute('dropoff_general_rate', '0.0')# 
        lidar_bp_16.set_attribute('points_per_second', '288000')# 每秒中生成的扫描点数
        lidar_bp_16.set_attribute('range', '200')# 最大探测距离（米）
        lidar_bp_16.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))# 旋转的频率
        
        lidar_16 = world.spawn_actor(lidar_bp_16, vehicle_trans_lidar, attach_to=None)
        
        lidar_16.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_16"))
        
        sensor_list.append(lidar_16)
               
        if SHOW:

            point_cloud = o3d.geometry.PointCloud()
            vis = o3d.visualization.Visualizer()
            vis.create_window(window_name='Wanji 64line Lidar', width=600, height=500, left=0, top=500, visible=True)

            ctr = vis.get_view_control()
            vis.add_geometry(point_cloud)

            extrinsic = np.eye(4)
            W = 600
            H = 500 
            camera_angle_x = 0.8457078337669373
            focal = 0.5 * W / np.tan(0.5 * camera_angle_x)
            # cx=(W-1)/2而不是W/2 !!!!!
            intrinsic = o3d.cuda.pybind.camera.PinholeCameraIntrinsic(width=W, height=H, fx=focal, fy=focal, cx=(W-1) / 2,cy=(H-1) / 2)
            param = o3d.cuda.pybind.camera.PinholeCameraParameters()
            param.extrinsic = extrinsic
            param.intrinsic = intrinsic
            ctr.convert_from_pinhole_camera_parameters(param)
            
        
            render_option = vis.get_render_option()
            render_option.point_size = 1.0
            render_option.background_color = np.asarray([0, 0, 0])  # 颜色 0为黑；1为白
            to_reset_view_point = True      
        # 持续消费消息
        while True:

            splicing = []

            world.tick()
            p_timestamp = world.get_snapshot().timestamp.platform_timestamp
            w_timestamp = get_time_stamp(p_timestamp)
           
  
            # 解析Kafka消息中的车辆位置信息
            messages = consumer.poll(timeout_ms=100)
        
            for paration in messages.values():
                for message in paration:
                        participants = message.value["participants"]
                        if len(participants) != 0:
                            errorids = []
                            nowids = []                       
                            #添加交通流
                            for par in participants:
                                nowids.append(par["ID"])

                                x, y = rule(par["longitude"], par["latitude"])
                                
                                trans = Transform(carla.Location(x=x, y=-y, z=37),
                                                    carla.Rotation(yaw= par["courseAngle"]-90))
                                
                                print(par["courseAngle"])
                                vehtype = par['type']
                                vehtype = 'vehicle.lincoln.mkz_2017'
                                id = par['ID']
                                
                                if id not in spawed_ids.keys():
                                    bp1 = world.get_blueprint_library().find(vehtype)
                                    try:
                                        bp1.set_attribute('color', '0,0,0')
                                    except:
                                        pass
                                    else:
                                        pass

                                    if vehtype == 'vehicle.lincoln.mkz_2017':
                                        batch = [
                                            carla.command.SpawnActor(bp1, trans).then(
                                                carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
                                        ]
                                        response = client.apply_batch_sync(batch, False)[0]
                                        if response.error:
                                            errorids.append(id)
                                        else:
                                            spawed_ids[id] = response.actor_id
                                    else:
                                        batch = [
                                            carla.command.SpawnActor(bp1, trans).then(
                                                carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
                                        ]
                                        response = client.apply_batch_sync(batch, False)[0]
                                        if response.error:
                                            errorids.append(id)
                                        else:
                                            spawed_ids[id] = response.actor_id
                    
                                else:
                                    if vehtype == 'vehicle.lincoln.mkz_2017':
                                        cid = spawed_ids[id]
                                        cactor = world.get_actor(cid)
                                        cactor.set_transform(trans)
                                    else:
                                        carlaid = spawed_ids[id]
                                        vehicle = world.get_actor(carlaid)
                                        if vehicle is not None:
                                            vehicle.set_transform(trans)
                            print("已有：", len(spawed_ids))

                            # 删除车辆
                            destodyed_ids = [id for id in spawed_ids if id not in nowids]
                            for did in destodyed_ids:
                                carlaid = spawed_ids[did]
                                vehicle = world.get_actor(carlaid)

                                if vehicle is not None:
                                    vehicle.destroy()
                                del spawed_ids[did]
                            print("更新：", len(nowids), "销毁:", len(destodyed_ids))

                
            s_frame, s_timestamp, s_name, s_data = sensor_queue.get(True, 1.0)
            splicing.append(_parse_lidar_cb(s_data))

            if splicing:
                concat_points = np.concatenate(splicing, axis=0)
                # concat_points = lidars
                concat_points[:, 1] = [-p for p in concat_points[:, 1]] #  将生成的点云Y值进行反转 由于carla的坐标系中Y值是反的
                if SAVE_PCD:
                    pcd_path = save_path + "/" + str(w_timestamp) + ".pcd"
                    points2pcd(pcd_path, concat_points)
                if SHOW:
                    points1 = np.array(concat_points)[:, :3]
                    colors1 = np.array(concat_points)[:, 3]
                    
                    point_cloud.points = o3d.utility.Vector3dVector(points1)
                    points_intensity = np.array(concat_points)[:, 3]  # intensity 
                    # print(points_intensity)
                    # r = 0
                    # b = 0
                    # g = 0
                    colors = []
                    for num in points_intensity:
                        # print(num)
                        intensity = num * 255
                        if intensity <= 33:
                            r = 0
                            b = 255
                            g = int(7.727 * intensity)
                        elif intensity >33 and intensity <= 66:
                            r = 0
                            b = int(255 - 7.727 * (intensity - 34))
                            g = 255
                        elif intensity > 66 and intensity <= 100:
                            r = int(7.727 * (intensity - 67))
                            b = 0
                            g = 255
                        elif  intensity > 100 and intensity <= 255:
                            r = 255
                            b = 0
                            g = int(255 - 7.727 * (intensity - 100)/4.697)
                        points_color = [r, g, b]
                        # points_color = [0, 0, 0.8]
                        colors.append(points_color)
                    # print(colors)
                    # 根据 intensity 为点云着色，由于变化不大，这里设置为蓝色[0,0,0.8]
                    point_cloud.colors = o3d.utility.Vector3dVector(colors)   
                    render_opt = vis.get_render_option()
                    render_opt.point_size = 1
                    render_opt.background_color = [0,0, 0]

                    box = o3d.geometry.AxisAlignedBoundingBox([-60,-60,-10],[60,60,20])
                    pcd_box = point_cloud.crop(box)
                    vis.update_geometry(point_cloud)
                    if to_reset_view_point:
                        vis.reset_view_point(True)
                        to_reset_view_point = False
                    vis.poll_events()
                    vis.update_renderer()


          

    finally:


        if synchronous_master:
            settings = world.get_settings()
            settings.synchronous_mode = False
            settings.fixed_delta_seconds = None
            world.apply_settings(settings)
        else:
            settings = world.get_settings()
            settings.synchronous_mode = False
            settings.fixed_delta_seconds = None
            world.apply_settings(settings)

        cv2.destroyAllWindows()
        for sensor in sensor_list:
            sensor.destroy()
        for actor in actor_list:
            if actor.is_alive:
                actor.destroy()
                

                
                    
     

if __name__ == "__main__":
    

    spawed_ids = {}
    actor_list, sensor_list = [], []
    save_path = '/home/tongjipc/carla/Dist/SDWR_UE4/PythonAPI/data'
    SHOW = True # 可视化展示雷达点云
    SAVE_PCD = True # 是否保存点云数据
    sensor_queue = Queue()
    
    try:
        main()
    except KeyboardInterrupt:
        print(' - Exited by user.')
